/*
	TOPCAT Payload Software, for use in the UKube-1 CubeSat mission.
	
	http://www.bath.ac.uk/elec-eng/invert/topcat.html
	
	Written by:
	Mark Jessop, University of Adelaide
	Talini Pinto Jayawardena, University of Bath
	
	Uses extracts from source code provided by Clyde Space for I2C communications.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    For a full copy of the GNU General Public License, 
    see <http://www.gnu.org/licenses/>.
*/

//////////////////////////////////
// OEMV-1DF Communication Code
//////////////////////////////////
//
// Created by        : Mark Jessop, 2011-07-26
//
//

#include <avr/pgmspace.h>
//
// GPS Defines
//
#define GPS_SYNC1  0xAA
#define GPS_SYNC2  0x44
#define GPS_SYNC3  0x12



//
// GPS Callback Function & Swing-Buffer.
//
//#define GPS_BUFFER_SIZE 512



gps_buffer gps_rx_1 = { { 0 }, 0, 0, 0, 0, 0 };
gps_buffer gps_rx_2 = { { 0 }, 0, 0, 0, 0, 0 };
gps_buffer * gps_current_buffer = &gps_rx_1; // Pointer to the buffer we're writing into
//gps_buffer * gps_packet = &gps_rx_1; // Pointer to the buffer we're directing the main loop to read from.
unsigned int gps_packet_length = 0;

unsigned char gps_header_match = 0;
unsigned char gps_prev_char = ' ';
unsigned int gps_local_counter = 0;
unsigned int gps_local_counter_2 = 0;
unsigned int expected_length;

void gps_isr(unsigned char c){
  
  unsigned int i = (unsigned int)(gps_current_buffer->ptr + 1);
  
  if( i != GPS_BUFFER_SIZE ){
     // Write the byte into the buffer.
     gps_current_buffer->buffer[gps_current_buffer->ptr] = c;
     gps_current_buffer->ptr = i;
     
     // Insert packet-decoding smarts here.
     
    // Header Hunting
    if( (gps_current_buffer->message_flags & 0x01) == 0){ // Have we already got a header?
		switch(c) {
		case 0x12:
			if(gps_prev_char == 0x44) gps_header_match++;
			break;
			
		case 0x44:
			if(gps_prev_char == 0xAA) gps_header_match++;
			break;
			
		case 0xAA:
			gps_header_match++;
			break;
		
		default:
			gps_header_match = 0;
			break;
		}
		if(gps_header_match == 3){
			gps_current_buffer->message_flags = 0x01;
			gps_local_counter = 0;
			gps_current_buffer->message_id = 0;
			expected_length = 0;
		}
	}
	
	// If we have found a header, start counting and extracting data from the packet.
	if( (gps_current_buffer->message_flags & 0x01) != 0){
		gps_local_counter++;
		
		switch(gps_local_counter){
		case(3): // LSB of the message ID.
			gps_current_buffer->message_id = (unsigned int)(c&0xFF);
			break;
			
		case(4): // MSB of the message ID.
			gps_current_buffer->message_id |= (unsigned int)(c&0xFF)<<8;
			msg_id = gps_current_buffer->message_id;
			break;
			
		case(7): // LSB of message length.
			expected_length = (unsigned int)(c&0xFF);
			break;
			
		case(8): // MSB of message length.
			expected_length |= (unsigned int)(c&0xFF)<<8;
			break;
			
		case(9): // Sequence Number LSB
			gps_current_buffer->seq_num = (unsigned int)(c&0xFF);
			break;
		
		default:
			break;
		}
		
		// Data extraction for specific messages
		if( (gps_current_buffer->message_id == 42) && (gps_current_buffer->message_flags != 0) ){ // BestPOS packet
			if(gps_local_counter == 91){ // Number of Sats (hopefully!)
				GPS_SATS = c;
				PAYLOAD_PARAMETER_2 = c;
				if(c>=4){ 
					bitSet(GPS_FLAGS,GPS_SATLOCK);
					digitalWrite(13,HIGH); // Debug!
				}
			}
		}
		
//		if( (gps_current_buffer->message_id == 41) && (gps_current_buffer->message_flags != 0) ){ // RAWEPHEM
//			if(gps_local_counter == 	
//		}
		
		if( (gps_local_counter>28) && (gps_local_counter == (expected_length + 4 + 26)) ){
		//if(gps_local_counter == 96){
			// We have reached the end of the packet. Switch buffers and raise flags.
			gps_current_buffer->message_length = gps_current_buffer->ptr;
			gps_current_buffer->ptr = 0; // Reset the pointer.
			
			// Set the pointer the main loop will be accessing.
    		gps_packet = gps_current_buffer;
			
		    // Switch buffers.
			if(gps_current_buffer == &gps_rx_1){
        		gps_current_buffer = &gps_rx_2;
    		}else if (gps_current_buffer == &gps_rx_2){
        		gps_current_buffer = &gps_rx_1;
    		}
    		// Clear the new buffer.
    		gps_current_buffer->message_flags = 0;
    		gps_current_buffer->message_id = 0;
    		gps_current_buffer->message_length = 0;
    		gps_current_buffer->ptr = 0;
    		
    		// Notify the main loop that data is available.
    		if(bitRead(GPS_FLAGS,GPS_MODESET)) bitSet(GPS_FLAGS,GPS_DATA_AVAILABLE);
    		
    		gps_local_counter = 0;
    		gps_header_match = 0;
		}
	}

  }else{
    
  // We have reached the end of the buffer or we have a full packet. Raise flag, and switch to the other buffer.
    gps_packet_length = gps_current_buffer->ptr;
    gps_current_buffer->ptr = 0; // Reset the pointer.
    
    // Set the pointer the main loop will be accessing.
    gps_packet = gps_current_buffer;
    
    // Switch buffers.
    if(gps_current_buffer == &gps_rx_1){
        gps_current_buffer = &gps_rx_2;
    }else if (gps_current_buffer == &gps_rx_2){
        gps_current_buffer = &gps_rx_1;
    }
    gps_current_buffer->message_flags = 0; // Clear message flags on the current buffer.
    gps_current_buffer->message_id = 0;
    gps_current_buffer->message_length = 0;
    // Write the current character to the first spot in the new buffer.
    gps_current_buffer->buffer[0] = c;
    gps_current_buffer->ptr = 1;
    
    
  	// Raise flags to inform main loop of new data
    if(bitRead(GPS_FLAGS,GPS_MODESET)) bitSet(GPS_FLAGS,GPS_DATA_AVAILABLE);
    
  }
  
  gps_prev_char = c;
}


//
// OEMV-1DF Initialization Functions & Strings.
//

char gps_tx_buffer[50];

// OEMV-1DF Initialization Commands
prog_char OEMV_INIT_0[] PROGMEM = "interfacemode com1 novatel novatel off\r\n";
prog_char OEMV_INIT_1[] PROGMEM = "interfacemode com2 novatel novatel off\r\n";
prog_char OEMV_INIT_2[] PROGMEM = "com com1 9600 n 8 1 n off on\r\n";
prog_char OEMV_INIT_3[] PROGMEM = "com com2 9600 n 8 1 n off on\r\n";
prog_char OEMV_INIT_4[] PROGMEM = "unlogall\r\n";
prog_char OEMV_INIT_5[] PROGMEM = "log com1 bestposb ontime 10.0\r\n";
prog_char OEMV_INIT_6[] PROGMEM = "ecutoff 0\r\n";

PROGMEM const char *OEMV_INIT_TABLE[] =  // 7 Commands
{
  OEMV_INIT_0, OEMV_INIT_1, OEMV_INIT_2, OEMV_INIT_3, OEMV_INIT_4, OEMV_INIT_5, OEMV_INIT_6
};

void oemv_startup(){
	// Clear GPS flags and variables.
	bitClear(GPS_FLAGS,GPS_SATLOCK);
  	bitClear(GPS_FLAGS,GPS_MODESET);
  	GPS_SATS = 0;
  	
	pinMode(PIN_3V3_SW, OUTPUT);
	digitalWrite(PIN_3V3_SW,LOW);
	GPS_PORT.begin(9600);
	GPS_PORT.setCallback(gps_isr);
	digitalWrite(PIN_3V3_SW,HIGH);
}

void oemv_init(){
    for( int i = 0; i<7; i++){ // CHANGE THIS LINE IF ADDING MORE COMMANDS.
      strcpy_P(gps_tx_buffer, (char*)pgm_read_word(&(OEMV_INIT_TABLE[i]))); 
      GPS_PORT.print( gps_tx_buffer );
      kick_watchdog();
    }
}


prog_char OEMV_M1_0[] PROGMEM = "log com1 rxstatuseventb onchanged\r\n";
prog_char OEMV_M1_1[] PROGMEM = "log com1 bestposb ontime 10.0\r\n";
prog_char OEMV_M1_2[] PROGMEM = "log com1 rangecmpb ontime 10.0\r\n";
prog_char OEMV_M1_3[] PROGMEM = "log com1 rawephemb onchanged\r\n";
prog_char OEMV_M1_4[] PROGMEM = "log com2 bestxyzb ontime 60.0\r\n";
prog_char OEMV_M1_5[] PROGMEM = "log com2 gprmc ontime 60.0\r\n";
prog_char OEMV_M1_6[] PROGMEM = "ecutoff 0\r\n";
prog_char OEMV_M2_0[] PROGMEM = "log com1 rxstatuseventb onchanged\r\n";
prog_char OEMV_M2_1[] PROGMEM = "log com1 bestposb ontime 20.0\r\n";
prog_char OEMV_M2_2[] PROGMEM = "log com1 rangecmpb ontime 20.0\r\n";
prog_char OEMV_M2_3[] PROGMEM = "log com1 rawephemb onchanged\r\n";

PROGMEM const char *OEMV_M1_TABLE[] = // 12 Commands
{
  OEMV_INIT_0, OEMV_INIT_1, OEMV_INIT_2, OEMV_INIT_3, OEMV_INIT_4, // These are the same as the Init commands.
  OEMV_M1_0, OEMV_M1_1, OEMV_M1_2, OEMV_M1_3, OEMV_M1_4, OEMV_M1_5, OEMV_M1_6
};

void oemv_mode1_set(){ 
	oemv_reset_buffers();
    for( int i = 0; i<12; i++){ // CHANGE THIS LINE IF ADDING MORE COMMANDS.
      strcpy_P(gps_tx_buffer, (char*)pgm_read_word(&(OEMV_M1_TABLE[i]))); 
      GPS_PORT.print( gps_tx_buffer );
      kick_watchdog();
    }
    
    ORBIT_MAX_MEM = (200*1024)/256;  //Assume 200KB max data per orbit 
    // Change mode flag here
    bitSet(GPS_FLAGS,GPS_MODESET);
}


PROGMEM const char *OEMV_M2_TABLE[] = // 12 Commands
{
  OEMV_INIT_0, OEMV_INIT_1, OEMV_INIT_2, OEMV_INIT_3, OEMV_INIT_4, // These are the same as the Init commands.
  OEMV_M2_0,OEMV_M2_1,OEMV_M2_2,OEMV_M2_3,
  OEMV_M1_4,OEMV_M1_5,OEMV_M1_6 // From Mode 1 Setup
};

void oemv_mode2_set(){  
	oemv_reset_buffers();
    for( int i = 0; i<12; i++){ // CHANGE THIS LINE IF ADDING MORE COMMANDS.
      strcpy_P(gps_tx_buffer, (char*)pgm_read_word(&(OEMV_M2_TABLE[i]))); 
      GPS_PORT.print( gps_tx_buffer );
      kick_watchdog();
    }
    ORBIT_MAX_MEM = (30*1024)/256; 
    // Change mode flag here
    bitSet(GPS_FLAGS,GPS_MODESET);
}

void oemv_shutdown(){
	digitalWrite(PIN_3V3_SW,LOW);
	
	// Boring, huh? Maybe I should put some cleanup code here.

}

void oemv_reset_buffers(){
	gps_rx_1.ptr = 0;
	gps_rx_1.message_flags = 0;
	gps_rx_1.message_length = 0;
	gps_rx_1.message_id = 0;
	gps_rx_1.seq_num = 0;
	gps_rx_2.ptr = 0;
	gps_rx_2.message_flags = 0;
	gps_rx_2.message_length = 0;
	gps_rx_2.message_id = 0;
	gps_rx_2.seq_num = 0;

	gps_header_match = 0;
	gps_prev_char = ' ';
	gps_local_counter = 0;
	gps_local_counter_2 = 0;
	expected_length = 0;
}


